#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from RobotBipe4T.Bipe4TRobot import Bipe4TRobot
import keyboard
import time

robotlib = BwRobotLib()
bipeRobot = Bipe4TRobot(robotlib)

#连接机器人设备
#robotlib.connectRobot("10.10.100.254",8899)
robotlib.connectRobot("192.168.1.107",55000)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)

#配置ID
bipeRobot.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#复杂双足
#-----------------------------------------------------------------------------

#处理键盘
def handleControlKeys(key):
    isDown = key.event_type == 'down'
    if not isDown:
        return

    cmd = key.name
    if(cmd == '1'):
        bipeRobot.goForward()        
    if(cmd == '2'):
        bipeRobot.goBack()
    if(cmd == '3'):
        bipeRobot.dance()
    if(cmd == '4'):
        bipeRobot.turnLeft()
    if(cmd == '5'):
        bipeRobot.turnRight()
    if(cmd == '6'):
        bipeRobot.slideLeft()
    if(cmd == '7'):
        bipeRobot.slideRight()
    bipeRobot.quickInit()
    pass


#控制提示
def controlTips():
    print("Keyboard to control :")
    print("1 : Go Forward")
    print("2 : Go Back")
    print("3 : Dance")
    print("4 : Turn Left")
    print("5 : Turn Right")
    print("6 : Run Left")
    print("7 : Run Right")

    print("-----------------------")
    print("0 :Exit")
    keyboard.hook(handleControlKeys)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    #helper.Wait(True)
    keyboard.wait('0')
    keyboard.unhook_all()

    pass

controlTips()